// --------------------------------------------------------------------------------------------------------------------
// <copyright file="MeshToSDF.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Geometry.SDF
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Numerics;

    /// <summary>
    /// Choose which face to cast rays from
    /// </summary>
    public enum Face
    {
        /// <summary>
        /// Cast from the face parallel to the Z-Y plane
        /// </summary>
        X,

        /// <summary>
        /// Cast from the face parallel to the Z-X plane
        /// </summary>
        Y,

        /// <summary>
        /// Cast from the face parallel to the
        /// </summary>
        Z
    }

    /// <summary>
    /// Methods that are used to compute a signed distance function from a mesh
    /// </summary>
    public struct MeshToSDF
    {
        /// <summary>
        /// Stamps a mesh from a file into a signed distance function
        /// </summary>
        /// <param name="sdf">The signed distance function to stamp</param>
        /// <param name="filename">The ASCII $ply$ file</param>
        public static void StampMeshFromFile(SDF sdf, string filename)
        {
            List<TriangleElement> triangles = MeshToSDF.ReadPLY(filename);
            MeshToSDF.ScanFromFace(sdf, triangles, Face.X);
            MeshToSDF.ScanFromFace(sdf, triangles, Face.Y);
            MeshToSDF.ScanFromFace(sdf, triangles, Face.Z);
        }

        /// <summary>
        /// Reads a ply file
        /// </summary>
        /// <param name="filename">The file to read</param>
        /// <returns>The triangles in the mesh</returns>
        private static List<TriangleElement> ReadPLY(string filename)
        {
            System.IO.StreamReader reader = new StreamReader(filename);

            int vertexCount = 0;
            int faceCount = 0;

            List<Vector3> vertices = new List<Vector3>();
            List<TriangleElement> triangles = new List<TriangleElement>();

            while (reader.EndOfStream == false)
            {
                string[] data = reader.ReadLine().Split(" ".ToCharArray());

                if (data[0] == "element" && data[1] == "vertex")
                {
                    vertexCount = Convert.ToInt32(data[2]);
                }
                else if (data[0] == "element" && data[1] == "face")
                {
                    faceCount = Convert.ToInt32(data[2]);
                }
                else if (data[0] == "end_header")
                {
                    break;
                }
            }

            for (int i = 0; i < vertexCount; i++)
            {
                string[] data = reader.ReadLine().Split(" ".ToCharArray());
                vertices.Add(new Vector3(Convert.ToDouble(data[0]), Convert.ToDouble(data[1]), Convert.ToDouble(data[2])));
            }

            for (int i = 0; i < faceCount; i++)
            {
                string[] data = reader.ReadLine().Split(" ".ToCharArray());
                triangles.Add(new TriangleElement(vertices[Convert.ToInt32(data[1])], vertices[Convert.ToInt32(data[2])], vertices[Convert.ToInt32(data[3])]));
            }

            reader.Close();

            return triangles;
        }

        /// <summary>
        /// Estimates the signed distance function when viewing the mesh from the specified face
        /// </summary>
        /// <param name="sdf">The contained for the signed distance function</param>
        /// <param name="triangles">The triangles used to estimate the signed distance function</param>
        /// <param name="face">Which face to select </param>
        private static void ScanFromFace(SDF sdf, List<TriangleElement> triangles, Face face)
        {
            int row = 0;
            int col = 0;
            int depth = 0;

            Vector3 dir = new Vector3();

            if (face == Face.Z)
            {
                row = sdf.Height;
                col = sdf.Width;
                depth = sdf.Depth;
                dir = new Vector3(0, 0, 1);
            }
            else if (face == Face.Y)
            {
                row = sdf.Width;
                col = sdf.Depth;
                depth = sdf.Height;
                dir = new Vector3(0, 1, 0);
            }
            else
            {
                row = sdf.Height;
                col = sdf.Depth;
                depth = sdf.Width;
                dir = new Vector3(1, 0, 0);
            }

            Parallel.For(
                0,
                row,
                (i) =>
                {
                    for (int j = 0; j < col; j++)
                    {
                        List<Tuple<double, Vector3>> intersectingTriangles = new List<Tuple<double, Vector3>>();
                        Vector3 point;

                        if (face == Face.Z)
                        {
                            point = sdf.Get3DPoint(j, i, 0);
                        }
                        else if (face == Face.Y)
                        {
                            point = sdf.Get3DPoint(i, 0, j);
                        }
                        else
                        {
                            point = sdf.Get3DPoint(0, i, j);
                        }

                        // get all triangles that the ray intersects with
                        for (int k = 0; k < triangles.Count; k++)
                        {
                            Tuple<double, Vector3> intersects = GeometricCalculations.RayTriangleInstersection(triangles[k], dir, point);
                            if (intersects != null)
                            {
                                intersectingTriangles.Add(intersects);
                            }
                        }

                        if (intersectingTriangles.Count != 0)
                        {
                            // sort triangles based on distance
                            intersectingTriangles.Sort(
                                delegate(Tuple<double, Vector3> x, Tuple<double, Vector3> y)
                                {
                                    if (Math.Abs(x.Item1) < Math.Abs(y.Item1))
                                    {
                                        return -1;
                                    }

                                    return 1;
                                });

                            intersectingTriangles = intersectingTriangles.Distinct().ToList();
                        }

                        int counter = 0;
                        for (int k = 0; k < depth; k++)
                        {
                            if (intersectingTriangles.Count != 0)
                            {
                                double prevDistance = counter == 0 ? double.MaxValue : intersectingTriangles[counter - 1].Item1 - (double)k * sdf.Resolution;
                                double nextDistance = counter == intersectingTriangles.Count ? double.MaxValue : intersectingTriangles[counter].Item1 - (double)k * sdf.Resolution;

                                if (nextDistance <= 0)
                                {
                                    counter++;
                                }

                                double val = Math.Min(Math.Abs(prevDistance), Math.Abs(nextDistance));
                                if (face == Face.Z)
                                {
                                    if (Math.Abs(sdf[j, i, k]) > Math.Abs(val) || double.IsNaN(sdf[j, i, k]))
                                    {
                                        sdf[j, i, k] = counter % 2 == 0 ? -val : val;
                                    }
                                }
                                else if (face == Face.Y)
                                {
                                    if (Math.Abs(sdf[i, k, j]) > Math.Abs(val) || double.IsNaN(sdf[i, k, j]))
                                    {
                                        sdf[i, k, j] = counter % 2 == 0 ? -val : val;
                                    }
                                }
                                else
                                {
                                    if (Math.Abs(sdf[k, i, j]) > Math.Abs(val) || double.IsNaN(sdf[k, i, j]))
                                    {
                                        sdf[k, i, j] = counter % 2 == 0 ? -val : val;
                                    }
                                }
                            }
                        }
                    }
                });
        }
    }
}
